// --------------------------------------------------------------------------------------------------------------------
// <copyright file="RobotPoseEstimationTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Navigation
{
    using System;   
    using System.Diagnostics;

    using Microsoft.Robotics.Navigation.Runtime.GroundTruthExtraction;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Vision.Runtime.GroundTruthExtraction;   
    using Microsoft.VisualStudio.TestTools.UnitTesting;      

    /// <summary>
    /// Robot pose estimation related tests.
    /// </summary>
    [TestClass]
    public class RobotPoseEstimationTest
    {
        /// <summary>
        /// Maximum tolerable error for robot pose measurement in meters.
        /// </summary>
        private const double MaxErrorInMeters = 0.02;

        /// <summary>
        /// Maximum tolerable error for robot pose measurement in radians
        /// </summary>
        private const double MaxErrorInRadians = 0.04;

        /// <summary>
        /// Test Landmark Points
        /// </summary>
        private static readonly Landmark[] LandmarkPoints = 
        {
            new Landmark(
                0,
                new Pose2D(0, 0, 0),
                new LandmarkTiltParams(-0.61019, 0.00518)),
           new Landmark(
                0,
                new Pose2D(16, 20, Math.PI),
                new LandmarkTiltParams(-0.61019, 0.00518))
        };

        /// <summary>
        /// Tests robot pose estimation by performing it on a reference robot offset and comparing the results for Pioneer 3D.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void RobotPoseEstimationReferenceTestPioneer()
        {   
            Pose2D[] referenceCameraPoses = 
            {
                new Pose2D(-0.0925, -0.0137, 0 * MathConstants.Degrees2Radians),
                new Pose2D(0.0211, -0.1226,  90 * MathConstants.Degrees2Radians),
                new Pose2D(-0.0002, 0.1078, -90 * MathConstants.Degrees2Radians),
                new Pose2D(0.1095, 0.002, 180 * MathConstants.Degrees2Radians),
                new Pose2D(-0.0925, -0.0137, 0 * MathConstants.Degrees2Radians),
                new Pose2D(-0.0925, -0.0137, 180 * MathConstants.Degrees2Radians),
             };

            Pose2D[] referenceRobotPoseActual = 
            {
                new Pose2D(-0.1, 0.005, 180 * MathConstants.Degrees2Radians), // This value is used in tilt calibration and robot calibration
                new Pose2D(0.005, -0.1, -90 * MathConstants.Degrees2Radians), // This value is used in robot calibration
                new Pose2D(-0.005, 0.1, 90 * MathConstants.Degrees2Radians),
                new Pose2D(0.1, -0.005, 0 * MathConstants.Degrees2Radians),  
                new Pose2D(16.093069837092877, 19.994959091173317, 0.0),
                new Pose2D(16.109734505934167, 20.019992081692568, 180 * MathConstants.Degrees2Radians),
            };

            int[] landmarkIds = 
            {
                0,
                0,
                0,
                0,
                1,
                1
            };           

            double[] distanceToLandmarks = 
            {
                2.097,
                2.094,
                2.095,
                2.095,
                2.097,
                2.097
            };

            RobotLandmarkCameraCalibrationInfo robotCalibrationInfoPioneer = new RobotLandmarkCameraCalibrationInfo(
                -3.07232,
                0.051808,
                0.100,
                -0.005,
                180 * MathConstants.Degrees2Radians);
            VerifyRobotPoseEstimation(
                 robotCalibrationInfoPioneer,
                 landmarkIds,
                 referenceCameraPoses,
                 referenceRobotPoseActual,
                 distanceToLandmarks);
        }

        /// <summary>
        /// Verifies the robot pose estimation for the given set of measured data
        /// </summary>
        /// <param name="robotCalibrationInfo">Robot calibration information</param>
        /// <param name="landmarkIds">Landmark ids corresponding to the landmarks under which the camera poses were measured</param>
        /// <param name="referenceCameraPoses">Measured camera Poses</param>
        /// <param name="referenceRobotPoseActual">Reference robot poses </param>
        /// <param name="distanceToLandmarks">Distance to the landmark (meters)</param>
        private static void VerifyRobotPoseEstimation(
            RobotLandmarkCameraCalibrationInfo robotCalibrationInfo,
            int[] landmarkIds, 
            Pose2D[] referenceCameraPoses,
            Pose2D[] referenceRobotPoseActual,
            double[] distanceToLandmarks)
        {
            RobotPoseEstimator robotPoseEstimator = new RobotPoseEstimator(robotCalibrationInfo);
            for (int i = 0; i < referenceRobotPoseActual.Length; i++)
            {
                Pose2D? robotPose = robotPoseEstimator.EstimateRobotPose(
                    referenceCameraPoses[i],
                    LandmarkPoints[landmarkIds[i]].Pose,
                    LandmarkPoints[landmarkIds[i]].TiltCalibrationParams,
                    distanceToLandmarks[i]);
                Assert.IsNotNull(robotPose);
                Assert.AreEqual(referenceRobotPoseActual[i].X, robotPose.Value.X, MaxErrorInMeters);
                Assert.AreEqual(referenceRobotPoseActual[i].Y, robotPose.Value.Y, MaxErrorInMeters);
                Assert.IsTrue(Math.Abs(CircularDifference(referenceRobotPoseActual[i].Heading, robotPose.Value.Heading)) < MaxErrorInRadians);
            }
        }

        /// <summary>
        /// Take the circular difference between the angles.
        /// </summary>
        /// <param name="rawHeading1">Raw Heading 1</param>
        /// <param name="rawHeading2">Raw Heading 2</param>
        /// <returns>Circular heading difference</returns>
        private static double CircularDifference(double rawHeading1, double rawHeading2)
        {
            double diff1 = Math.Abs(rawHeading1 - rawHeading2);
            double diff2 = Math.Abs((rawHeading1 + MathConstants.TwoPI) - rawHeading2);
            double diff3 = Math.Abs((rawHeading2 + MathConstants.TwoPI) - rawHeading1);
            return Math.Min(Math.Min(diff1, diff2), diff3);
        }
    }
}
